;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;           constrained optimization(带约束的最优化问题)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;;;; **A** optimization with strict-inequality constraints(约束形式仅含*严格*不等式约束)
;; Problem:
;;               min f(x) where x in R^n and c_i(x) > 0, i=1,..,I.

;; Method: construct a seq of x: x0, x1 = x0 + s0 * d0, x2 = x1 + s1 * d1, ...

;; Key point: constraint-functions c_i(x) are strict-inequalities which means
;; the allowed region of points is a **open set**.


#!r6rs
(library-directories '("." "/home/cl/gits/libs-for-chezscheme"))
(import (linear-algebra)
        (stream)
        (range)
        (gnuplot))


;;next-point
(define (next-point x s d) (vector-add x (scalar-*-vector s d)))        

;; get-step function
(define (get-step f grad constraint? epsilon-x c1 c2)                     ;get-step
  ;; objective-function: f(x), grad(x): grad of f, constraint?(x): constraint-function
  ;; if x satisfies the constraint, (constraint? x)=#t, else #f
  ;; epsilon-x: minimun of distance of two neighbor points
  (lambda (x d default-step)
    (define (y s) (f (next-point x s d)))
    (define (dy s) (vector-dot d (grad (next-point x s d))))
    (define (in-region? s) (constraint? (next-point s d)))
    (if default-step
        (line-search-algorithm y dy in-region? default-step 'any c1 c2)   ;line-search-algorithm
        (let ([min-step (* 10 (/ epsilon (vector-norm/inf d)))])
          (line-search-algorithm y dy in-region? #f min-step c1 c2)))))


(define (next-point-in-constraint x d s constraint?)
  (let ([new-x (next-point x s d)])
    (if (constraint? new-x) new-x
        (begin (printf " -- face-constraint --")
               (next-point-in-constraint x d (* 0.8 s) constraint?)))))

                                        ; Armijo condition (ac?)
(define (armijo-condition? c1)
  (lambda (y0 y1 dy0 s)
    ;;y0 = y(0), others like
    (<= y1 (+ y0 (* c1 s dy0)))))
                                        ; curvature condition (cc?)
(define (curvature-condition? c2)
  (lambda (dy0 dys)
    ;;dy0 = dy(0), dys = dy(s)
    (<= (abs dys) (* c2 (abs dy0)))))

;; --- line search algorithm
;; we use Strone Wolfe rule(i.e. ac? + cc?)
(define (line-search-algorithm y dy in-region? default-step min-step c1 c2)
  ;; assume: 0 < c1 < c2 <1, dy(0) < 0
  ;; if given default-step, use default-step, else use min-step to construct step-size
  ;; return: some s in-region that satisfies strong-wolfe-rule.
  (let ([y0 (y 0)] [dy0 (dy 0)])
    (define (sdc? s s0)                     ;sdc?(sufficient-desceant-condition, extend ac?)
      (lambda (ys ys0)
        (and ((armijo-condition? c1) y0 ys dy0 s) (<= ys ys0))))
    (define (cc? s)                         ;cc?
      (lambda (dys)
        ((curvature-condition? c2) dy0 dys)))
    (define (get-allowed-step s)            ;get-allowed-step
      (if (in-region? s) s
          (begin (printf " --face-constraint-- ")
                 (get-allowed-step (* 0.8 s)))))
    (define step-size                       ;step-size
      (get-allowed-step
       (if default-step default-step
           (let ([ddy0 (/ (- (dy  min-step) dy0) min-step)])
             (if (positive? ddy0) (- (/ dy0 ddy0)) min-step)))))
    (define (increase-step s) (get-allowed-step (+ s step-size)))  ;increase-step
    
    ;;ZOOM FUNCTION
    (define (zoom current bound)
      ;; current satisfies sdc?, but not cc?;
      ;; make sure that there must be one s between current and bound.
      (lambda (yc dyc yb)
        (let* ([s (get-allowed-step (inner-point current bound yc dyc yb))];;是否需要get-allowed-step
               [ys (y s)]
               [dys (dy s)])
          ;;(display "  **zoom**  ")
          (if ((sdc? s current) ys yc)
              (cond [((cc? s) dys) s] ;ok
                    [(negative? (* dys (- bound current))) ((zoom s bound) ys dys yb)]
                    (else ((zoom s current) ys dys yc)))
              ((zoom current s) yc dyc ys)))))
    (define (inner-point c b yc dyc yb)           ; inner-point in c and b
      (let ([aux (/ (+ b c) 2 (- 1 (/ (- yb yc) (- b c) dyc)))])
        (if (or (nan? aux) (<= (* (- aux c) (- b aux)) 0))
            (/ (+ b c) 2) aux)))
    ;; MAIN FUNCTION
    ;; loop to get an increasing seq: s0 = 0 < s1 < s2 < ...,
    ;; until one can satisfy strong-wolfe-rule, one exception is s_i may be too large.
    (if (and ((sdc? 0 0) y0 y0) ((cc? 0) dy0))
        0 ;ok
        (let ([s1 step-size])
          (let loop ([s0 0] [ys0 y0] [dys0 dy0]
                     [s1 s1] [ys1 (y s1)])
            ;;require: y'(s0) < 0 and s0 < s1
            ;;(display "  --loop--  ")
            (if ((sdc? s1 s0) ys1 ys0)
                (let ([dys1 (dy s1)])
                  (cond [((cc? s1) dys1) s1] ;ok
                        [(negative? dys1)
                         (let ([new-s1 (increase-step s1)])
                           (loop s1 ys1 dys1 new-s1 (y new-s1)))]
                        (else ((zoom s1 s0) ys1 dys1 ys0)))) ;ok
                ((zoom s0 s1) ys0 dys0 ys1)))))) ;ok
  );; end line-search


;;; find direction

                                        ; descent-direction?
(define (descent-direction? d g epsilon)
  (< (/ (vector-dot d g) (vector-norm d) (vector-norm g)) (- epsilon)))

;; (B1) (SD) Steepest-Descent method 
(define (SD g1)
  ;; return: d1 = -g1
  (scalar-*-vector -1 g1))

;; (B2) (Sub) Subspace method
(define (subspace grad constraint? f0 ng0 ug0 nd0 ud0 x1 f1 ng1 ug1 min-step)
  ;; grad-function; x0's info:f0,g0; d0=x1-x0; x1's info:f1,g1;
  ;; nv: norm of v, uv: unit-vector of v.
  ;; min-step: minimun-distance between two points(as input).
  ;; return: (list nd1 ud1 x2)
  (define (generate-xs)   ;;generate xs to help constructing Hessian
    (let loop ([s min-step])
      (let ([ds (scalar-*-vector (- s) ug1)]
            [xs (vector-add x1 ds)])
        (if (constraint? xs) (list s ds xs)
            (loop (* 0.8 s))))))
  (let* ([dots (map vector-dot
                    ;;unit-vec d0g0  d0g1 g0g1
                    (list       ud0  ud0  ug0)
                    (list       ug0  ug1  ug1))]
         [s-ds-xs (generate-xs)]
         [s (car s-ds-xs)] [ds (cadr s-ds-xs)] [xs (caddr s-ds-xs)]
         [gs (grad xs)] [ngs (vector-norm gs)] [ugs (scalar-*-vector (/ 1 ngs) gs)]
         [g1-by-d0 (/ ng1 nd0)] [g0-by-g1 (/ ng0 ng1)] [gs-by-g1 (/ ngs ng1)] [g1-by-ds (/ ng1 s)]
         [d0Gd0 (* g1-by-d0 (- (list-ref dots 1) (* g0-by-g1 (list-ref dots 0))))]
         [d0Gg1 (* g1-by-d0 (- 1 (* g0-by-g1 (list-ref dots 2))))]
         [g1Gg1 (* g1-by-ds (- 1 (* gs-by-g1 (vector-dot ug1 ugs))))]
         [is-G-invertible? (positive? (- (* d0Gd0 g1Gg1) (expt d0Gg1 2)))])
    ;;key-part: 双曲函数和椭圆函数皆可以求得区域极小值!! xxxxxx
    (if is-G-invertible?
        (let* ([m-at-x0 (+ f1 (* -1 nd0 ng1 (list-ref dots 1)) (* 1/2 d0Gd0 (expt nd0 2)))]
               [er (let ([aux (abs (/ m-at-x0 f0))])
                     (- 1 (max aux (/ 1 aux))))]
               [R (cond [(< er 0) (* 1/2 nd0)]
                        [(< er 0.5) nd0]
                        (else (* 2 nd0)))])
          ;;(printf "er= ~a, R=~a;\n" er R)
          (let* ([beta (/ (- (* (list-ref dots 1) g1Gg1) d0Gg1)
                          (- (* (list-ref dots 1) d0Gg1) d0Gd0))]
                 [pre-s (* ng1 (/ (- 1 (* beta (list-ref dots 1)))
                                  (+ g1Gg1 (* -2 beta d0Gg1) (* beta beta d0Gd0))))]
                 [dir (scalar-*-vector pre-s (vector-minus (scalar-*-vector beta ud0) ug1))]
                 [ndir (vector-norm dir)] [udir (scalar-*-vector (/ 1 ndir) dir)])
            (if (and (positive? a) (descent-direction? udir ug1))
                (list (abs (* pre-s ndir)) udir
                      (vector-add x1 (scalar-*-vector pre-s dir)))
                (begin (printf "use ** sd ** ")
                       (list min-step (scalar-*-vector -1 ug1) xs)))))
        (begin (printf "USE ** SD ** ")
               (list s (scalar-*-vector -1 ug1) xs)))));;ok

;;; Collect line-search-algorithm and find-direction methods
;; SD-method
(define (SD-method f grad constraint? x-init epsilon-x epsilon-g c1 c2)
  (define (SD-direction x) (SD (grad x)))
  (define (SD-step x d) ((get-step f grad constraint? epsilon-x c1 c2) x d #f))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let loop ([x x-init])
    (let* ([d (SD-direction x)]
           [s (SD-step x d)]
           [next-x (next-point x s d)])
      ;;(display (f next-x)) (newline)
      (if (< (vector-norm (grad next-x)) epsilon-g) ;;终止条件
          next-x ;=>
          (loop next-x)))))

;;
(define (Subspace/with-SD-method f grad constraint? x-init epsilon-x epsilon-g c1 c2)
  (define (SD-direction g) (SD g))
  (define step-function (get-step f grad constraint? epsilon c1 c2))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let* ([x0 x-init] [g0 (grad x0)]
         [d0 (SD-direction g0)] [s0 (step-function x0 d0 #f)]
         [x1 (next-point x0 s0 d0)] [g1 (grad x1)])    
    (let loop ([x0 x0] [g0 g0]
               [d0 d0] [s0 s0]
               [x1 x1] [g1 g1])
      (if (< (vector-norm g1) epsilon-g) x1 ;ok
          (let* ([ng0 (vector-norm g0)] [ug0 (scalar-*-vector (/ 1 ng0) g0)]
                 [nd0 (vector-norm d0)] ;;;xxxxx
                 [sub (subspace grad constraint? f0 ng0 ug0 nd0 ud0 x1 f1 ng1 ug1 epsilon-x)])
            (if (and sub (descent-direction? (cdr sub) g1 epsilon))
                (let* ([s1-Sub (step-function x1 (cdr sub) (car sub))]
                       [x2-Sub (next x1 s1-Sub (cdr sub))])
                  (loop x1 g1 (cdr sub) s1-Sub x2-Sub (grad x2-Sub)))
                (let* ([d1-SD (SD-direction g1)]
                       [s1-SD (step-function x1 d1-SD #f)]
                       [x2-SD (next x1 s1-SD d1-SD)])
                  ;;(display sub) (newline);; 看得出主要是G做不到正定性
                  (display "\n**in <Sub>, SD is used**\n")
                  (loop x1 g1 d1-SD s1-SD x2-SD (grad x2-SD))))))
      )))

